#ifndef _OMNI_WHEEL_CAL_H
#define _OMNI_WHEEL_CAL_H

#include "M3508.h"
extern DJI_Motor_ControlTypeDef M3508_Motor;
#define PI 3.1415926f

/*
* 全向轮底盘状态
*    0     3
*	 1     2
* 
*    self_pos
*          X
*          | 
*       z<-|
*   Y<------ 
* 
*   world_pos 
*   Y   
*   | 
*   |<-z
*   ------>X
* 
*/

/**
 * @brief 位置信息
 * @param x x坐标,m
 * @param y y坐标,m
 * @param yaw 航向角,rad
 */
typedef struct
{
	float x;   // m
	float y;   // m
	float yaw; // degree(rad)
} Position;  // unit: m, rad

/**
 * @brief 速度信息
 * @param Vx x方向速度,m/s
 * @param Vy y方向速度,m/s
 * @param Vw 航向角速度,rad/s
 * @note Robot.Robot_in_self.velocity_exp. Robot为Robot_INFO变量
 */
typedef struct
{
	float Vx;
	float Vy;
	float Vw;
} Velocity; // unit: m/s, rad/s

/**
 * @brief 加速度信息
 * @param Ax x方向加速度,m/s^2
 * @param Ay y方向加速度,m/s^2
 * @param Aw 航向角加速度,rad/s^2
 */
typedef struct
{
	float Ax;
	float Ay;
	float Aw;
} Acceleration; // unit: m/s^2, rad/s^2

/**
 * @brief 机器人位置信息
 * @param position_now 当前位置信息
 * @param velocity_now 当前速度信息
 * @param position_exp 期望位置信息
 * @param velocity_exp 期望速度信息
 * @param position_last 上一次位置信息
 * @param velocity_last 上一次速度信息
 * @param acceleration_now 当前加速度信息
 *
 */
typedef struct
{
	Position position_now;
	Velocity velocity_now;
	Position position_exp;
	Velocity velocity_exp;
	Position position_last;
	Velocity velocity_last;
	Acceleration acceleration_now;
} Wheel_Pos;

/**
 * @brief 机器人信息
 * @param Robot_in_self 机器人在自身坐标系下的位置信息
 * @param Robot_in_world 机器人在世界坐标系下的位置信息
 * @param Robot_Contorl_Mode 机器人控制模式
 * @param Wheel_Speed 机器人轮子速度
 *
 */
typedef struct
{
	Wheel_Pos Robot_in_self;
	Wheel_Pos Robot_in_world;
	uint8_t Robot_Contorl_Mode;
	float Wheel_Speed[4];
	float Wheel_Speed_Abs[4];
	float Wheel_Direction[4];
	float Robot_Speed_max;
} Wheel_Data;


void Robot_Velocity_To_Wheel_Speed(Wheel_Data* robot);
void Robot_Velocity_World_To_Self(Wheel_Data* robot);
void World_To_Self(Wheel_Data* robot);
void Wheel_Data_To_Motor(Wheel_Data* robot,DJI_Motor_ControlTypeDef* motor);

#endif
